//
// Created by Scave on 2024/2/25.
//
#include <stdexcept>
#include <cmath>
#include "bx/math.h"
#include "bgfx/bgfx.h"
#include "Supposition.h"

NS_RIDER_BEGIN

    const Color Colors::Red = Color{1, 0, 0, 1};
    const Color Colors::Green = Color{0, 1, 0, 1};
    const Color Colors::Blue = Color{0, 0, 1, 1};
    const Color Colors::Black = Color{0, 0, 0, 1};
    const Color Colors::White = Color{1, 1, 1, 1};
    const Color Colors::Orange = Color{1, 1, 0, 1};
    const Color Colors::Purple = Color{1, 0, 1, 1};
    const Color Colors::Cyan = Color{0, 1, 1, 1};
    const Vec3 Vectors::Forward = {0, 0, 1};
    const Vec3 Vectors::Back = {0, 0, -1};
    const Vec3 Vectors::Left = {-1, 0, 0};
    const Vec3 Vectors::Right = {1, 0, 0};
    const Vec3 Vectors::Up = {0, 1, 0};
    const Vec3 Vectors::Down = {0, -1, 0};
    const Vec3 Vectors::Invalid = {2147483647.0, 2147483647.0, 2147483647.0};


    NdcPoint NdcPoint::Add(const NdcPoint& a, const NdcPoint& b) {
        return {
                a.x + b.x,
                a.y + b.y,
                a.z + b.z
        };
    }

    NdcPoint NdcPoint::Sub(const NdcPoint& a, const NdcPoint& b) {
        return {
                a.x - b.x,
                a.y - b.y,
                a.z - b.z
        };
    }

    NdcPoint NdcPoint::Scalar(const NdcPoint& a, const float b) {
        return {
                a.x * b,
                a.y * b,
                a.z * b
        };
    }

    float NdcPoint::Dot(const NdcPoint& a, const NdcPoint& b) {
        return a.x * b.x + a.y * b.y + a.z * b.z;
    }

    NdcPoint NdcPoint::Cross(const NdcPoint& a, const NdcPoint& b) {
        return {
                a.y * b.z - a.z * b.y,
                a.z * b.x - a.x * b.z,
                a.x * b.y - a.y * b.x
        };
    }

    NdcPoint NdcPoint::Normalize(const NdcPoint& a) {
        float length = NdcPoint::Length(a);
        if (length == 0.0f) {
            throw std::invalid_argument("Can't normalize a zero-length vector");
        } else {
            return NdcPoint::Scalar(a, 1.0 / length);
        }
    }

    float NdcPoint::Length(const NdcPoint& a) {
        return std::sqrt(a.x * a.x + a.y * a.y + a.z * a.z);
    }

    float NdcPoint::Distance(const NdcPoint& a, const NdcPoint& b) {
        NdcPoint c = NdcPoint::Sub(a, b);
        return NdcPoint::Length(c);
    }

    NdcPoint NdcPoint::Center(const NdcPoint& a, const NdcPoint& b) {
        return {
                (a.x + b.x) / 2,
                (a.y + b.y) / 2,
                (a.z + b.z) / 2
        };
    }

    Mat4::Mat4(): array16{} {
        bx::mtxIdentity(array16.data());
    }

    Mat4::Mat4(float* data) {
        std::copy(data, data + 16, this->array16.begin());
    }

    void Mat4::Scale(const Vec3& scale) {
        Scale(scale.x, scale.y, scale.z);
    }

    void Mat4::Scale(float sx, float sy, float sz) {
        float mtx_scale[16];
        bx::mtxScale(mtx_scale, sx, sy, sz);
        bx::mtxMul(array16.data(), array16.data(), mtx_scale);
    }

    void Mat4::Rotate(const Quaternion& q) {
        float mtx_rotation[16];
        bx::mtxIdentity(mtx_rotation);
        bx::mtxFromQuaternion(mtx_rotation, {q.x, q.y, q.z, q.w});
        bx::mtxMul(array16.data(), array16.data(), mtx_rotation);
    }

    void Mat4::Rotate(float rx, float ry, float rz) {
        float mtx_rotation[16];
        bx::mtxRotateXYZ(mtx_rotation, rx, ry, rz);
        bx::mtxMul(array16.data(), array16.data(), mtx_rotation);
    }

    void Mat4::Translate(const Vec3& translation) {
        Translate(translation.x, translation.y, translation.z);
    }

    void Mat4::Translate(float tx, float ty, float tz) {
        float mtx_translation[16];
        bx::mtxTranslate(mtx_translation, tx, ty, tz);
        bx::mtxMul(array16.data(), array16.data(), mtx_translation);
    }

    void Mat4::SRT(const Vec3& scale, const Quaternion& rotation, const Vec3& translation) {
        float mtx_translation[16], mtx_scale[16], mtx_rotate[16];
        bx::mtxIdentity(mtx_rotate);
        bx::mtxTranslate(mtx_translation, translation.x, translation.y, translation.z);
        bx::mtxFromQuaternion(mtx_rotate, {rotation.x, rotation.y, rotation.z, rotation.w});
        bx::mtxScale(mtx_scale, scale.x, scale.y, scale.z);
        bx::mtxMul(array16.data(), array16.data(), mtx_translation);
        bx::mtxMul(array16.data(), array16.data(), mtx_rotate);
        bx::mtxMul(array16.data(), array16.data(), mtx_scale);
    }

    void Mat4::LookAt(const Vec3& eye, const Vec3& at, const Vec3& up) {
        bx::Vec3 bx_eye = {eye.x, eye.y, eye.z};
        bx::Vec3 bx_at = {at.x, at.y, at.z};
        bx::Vec3 bx_up = {up.x, up.y, up.z};
        bx::mtxLookAt(array16.data(), bx_eye, bx_at, bx_up);
    }

    void Mat4::Perspective(float fov, float aspect, float near, float far) {
        bx::mtxProj(array16.data(), fov, aspect, near, far, bgfx::getCaps()->homogeneousDepth);
    }

    void Mat4::Orthographic(float left, float right, float bottom, float top, float near, float far, float offset) {
        bx::mtxOrtho(array16.data(), left, right, bottom, top, near, far, offset, bgfx::getCaps()->homogeneousDepth);
    }

    Mat4 Mat4::Mul(const Mat4& other) {
        bx::mtxMul(array16.data(), array16.data(), other.array16.data());
        return *this;
    }

    Mat4 Mat4::Inverse() {
        bx::mtxInverse(array16.data(), array16.data());
        return *this;
    }

    Mat4 Mat4::Transpose() {
        bx::mtxTranspose(array16.data(), array16.data());
        return *this;
    }

    const float* Mat4::Data() const {
        return array16.data();
    }

    bool Mat4::Equals(const Mat4& other, float tolerance) const {
        if (&array16 == &other.array16) {
            return true;
        }
        for (int i = 0; i < 16; ++i) {
            if (std::fabs(array16[i] - other.array16[i]) > tolerance) {
                return false;
            }
        }
        return true;
    }

    bool Mat4::operator ==(const Mat4& other) const {
        return Equals(other);
    }

    bool Mat4::operator !=(const Mat4& other) const {
        return !Equals(other);
    }

    Quaternion Quaternion::FromEuler(float pitch, float yaw, float roll) {
        // 将角度转换为弧度
        double p_rad = pitch * M_PI / 180.0;
        double y_rad = yaw * M_PI / 180.0;
        double r_rad = roll * M_PI / 180.0;

        // 计算半角
        double halfP = p_rad * 0.5;
        double halfY = y_rad * 0.5;
        double halfR = r_rad * 0.5;

        // 计算三角函数值
        float sinP = std::sin(halfP);
        float cosP = std::cos(halfP);
        float sinY = std::sin(halfY);
        float cosY = std::cos(halfY);
        float sinR = std::sin(halfR);
        float cosR = std::cos(halfR);

        Quaternion q_pitch = {cosP, sinP, 0.0, 0.0};
        Quaternion q_yaw = {cosY, 0.0, sinY, 0.0};
        Quaternion q_roll = {cosR, 0.0, 0.0, sinR};
        return (q_roll.Mul(q_pitch).Mul(q_yaw)).Normalized();
    }

    float Quaternion::Norm() const {
        return std::sqrt(w * w + x * x + y * y + z * z);
    }

    Quaternion Quaternion::Normalized() const {
        float len = Norm();
        if (len < 1e-12) return *this; // 防止除零
        return {w / len, x / len, y / len, z / len};
    }

    Quaternion Quaternion::Conjugate() const {
        return Quaternion {w, -x, -y, -z};
    }

    Quaternion Quaternion::Mul(const Quaternion& other) const {
        return Quaternion {
                w * other.w - x * other.x - y * other.y - z * other.z, // w
                w * other.x + x * other.w + y * other.z - z * other.y, // x
                w * other.y - x * other.z + y * other.w + z * other.x, // y
                w * other.z + x * other.y - y * other.x + z * other.w  // z
        };
    }

    Quaternion& Quaternion::AddRotation(float pitch, float yaw, float roll) {
        *this = this->Mul(Quaternion::FromEuler(pitch, yaw, roll));
        return *this;
    }

    Vec3 Quaternion::ToDirection() const{
        // 在右手坐标系中，默认的光源方向是-Z轴（指向屏幕内）
        Vec3 default_direction = {0.0, 0.0, -1.0};
        Quaternion default_quat = {0.0, default_direction.x, default_direction.y, default_direction.z};
        Quaternion q = Normalized();
        // 计算旋转: q * v * q⁻¹
        Quaternion tmp = q.Mul(default_quat);
        Quaternion result = tmp.Mul(q.Conjugate());
        // 提取旋转后的向量部分
        return Vec3::Normalize({result.x, result.y, result.z});
        /*// 第二种算法
        Quaternion q_norm = Normalized();
        float rx = 2 * (q_norm.x * q_norm.z + q_norm.w * q_norm.y);
        float ry = 2 * (q_norm.y * q_norm.z - q_norm.w * q_norm.x);
        float rz = 1 - 2 * (q_norm.x * q_norm.x + q_norm.y * q_norm.y);
        return Vec3::Normalize({rx, ry, rz});*/
    }

    Vec3 Quaternion::ToEuler() const {
        Vec3 euler;
        float qw = w, qx = x, qy = y, qz = z;
        float sinP = 2.0f * (qw * qx + qy * qz);
        if (std::fabs(sinP) >= 0.9999f) {
            // 万向锁情况处理
            euler.x = (sinP > 0 ? 90.0f : -90.0f) * (sinP > 0 ? 1.0f : -1.0f);
            euler.y = std::atan2(2.0f * (qw * qy - qz * qx),
                                   1.0f - 2.0f * (qy * qy + qz * qz)) * 180.0f / M_PI;
            euler.z = 0.0f; // 在万向锁时，Roll角度失去意义
        } else {
            // 正常情况
            euler.x = std::asin(sinP) * 180.0f / M_PI;
            euler.y = std::atan2(2.0f * (qw * qy - qz * qx),
                                   1.0f - 2.0f * (qx * qx + qy * qy)) * 180.0f / M_PI;
            euler.z = std::atan2(2.0f * (qw * qz - qx * qy),
                                    1.0f - 2.0f * (qx * qx + qz * qz)) * 180.0f / M_PI;
        }
        return euler;
    }


NS_RIDER_END
